#include <cstdlib>
#include <cstdio>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Empty.h>
#include <sensor_msgs/Joy.h>
#include <cmath>
#include <ros/time.h>
#include "teleop.h"

#define BUTTONWAIT		0.1 //button wait time (sec)

// XBOX360 Controller = 0x045e028e
#define XBOX360_ID		"045e028e"

// Set Buttons
#define BUTTON_A		0	//Takeoff
#define BUTTON_B		1	//Land
#define BUTTON_X		2	//Change Camera
#define BUTTON_Y		3
#define BUTTON_LB 		4
#define BUTTON_RB 		5
#define BUTTON_BACK		6
#define BUTTON_START	7
#define BUTTON_XBOX		8	//reset
#define BUTTON_LS		9	// Press Left Stick in
#define BUTTON_RS		10	// Press Right Stick in

// DPAD Axes
#define DPAD_X			6	// -1 == left, 1 == right
#define DPAD_Y			7	// -1 == up,   1 == down

// Set Trigger Axis
#define TRIG_LT			2
#define TRIG_RT			5 

// Set Stick Axis
#define STICK_LS_X		0	//Yaw
#define STICK_LS_Y		1	//Height
#define STICK_RS_X		3	//Roll
#define STICK_RS_Y		4	//Pitch


//Logitech Extreme 3D Pro Joystick = 0x046dc215
#define LOGITECH_ID 	"046dc215"

// Set Buttons
#define BUTTON_TRIG		0	
#define BUTTON_THUMB	1	//Change Camera
#define BUTTON_3		2	//takeoff
#define BUTTON_4		3	//land
#define BUTTON_5 		4	//reset
#define BUTTON_6 		5	
#define BUTTON_7		6
#define BUTTON_8		7
#define BUTTON_9		8
#define BUTTON_10		9
#define BUTTON_11		10
#define BUTTON_12		11	

// DPAD Axes
#define JOY_DPAD_X		4	// +1 == left, -1 == right
#define JOY_DPAD_Y		5	// +1 == up,   -1 == down

// Set Stick Axis
#define AXIS_X			0	//Roll
#define AXIS_Y			1	//Pitch
#define AXIS_THETA		2	//Yaw
#define AXIS_LEVER		3	//Alitude


using namespace std;

// sets up controller mappings for detected device/controller. Also sets up deadzone.
void TeleopArdrone::mapController(){
  id = getDeviceId("js0", "Vendor", "Product"); // Returns string of Vendor + Product ID of device on /dev/input/js0

  if (id == LOGITECH_ID){		// if Logitech Joystick detected then map Joystick Axis/Buttons
	land = BUTTON_3;
  	reset = BUTTON_5;
  	takeoff = BUTTON_4;
	togglecam = BUTTON_THUMB;
	yaw = AXIS_THETA;
	altitude = AXIS_LEVER;
	roll = AXIS_X;
	pitch = AXIS_Y;
	deadzone = 0.04;
	alt_deadzone = 0.00;
	deadman_sw = BUTTON_TRIG;
	
	alt_up = -1;
	alt_down = -1;
	pitch1 = -1;
  }
  
  else if (id == XBOX360_ID) {	// if Xbox 360 controller is detected then map Xbox360 Axis/Buttons
	land = BUTTON_B;
  	reset = BUTTON_XBOX;
  	takeoff = BUTTON_X;
	togglecam = BUTTON_A;
	roll = STICK_LS_X;
	alt_up = TRIG_RT;
	alt_down = TRIG_LT;
	yaw = STICK_RS_X;
	pitch = STICK_RS_Y;
	//pitch1 = STICK_LS_Y;
	deadzone = 0.0;
	alt_deadzone = 0;
	
	pitch1 = -1;
	altitude = -1;
	deadman_sw = -1;
  }
  
  else {
  	// new controller which will do nothing
  	// init vals to 0 to prevent segFault
  	land = 0;
  	reset = 0;
  	takeoff = 0;
	togglecam = 0;
	roll = 0;
	altitude = 0;
	yaw = 0;
	pitch = 0;
	deadzone = 0;
	alt_deadzone = 0;
	alt_up = 0;
	alt_down = 0;
	pitch1 = 0;
  }
}


TeleopArdrone::TeleopArdrone(){  
  mapController();	//map controller buttons/axis
  
  // These settings/mapping are used if axes are not defined in .launch file??
  //linear_x = pitch;	// maps linear_x to pitch axis (pitch and others assigned in mapController)
  //linear_y = yaw;	// maps linear_y to yaw axis
  //linear_z = altitude;	// maps linear_z to altitude axis
  //angular_ = roll;		// maps angular_ to roll axis, can be considered strafing
  
  // taken from basic ROS Ardrone app
  // publishes command to various ROS channels
  pub = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
  land_pub = nh_.advertise<std_msgs::Empty>("/ardrone/land", 1);
  reset_pub = nh_.advertise<std_msgs::Empty>("/ardrone/reset", 1);
  takeoff_pub = nh_.advertise<std_msgs::Empty>("/ardrone/takeoff", 1);
  toggleCam_pub = nh_.advertise<std_msgs::Empty>("/ardrone/togglecam", 1);
  joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &TeleopArdrone::joyCallback, this);
  
  // overrides mapController mappings, from launch file
  //nh_.param("linearx", linear_x, linear_x);		// axis select in launch file
  //nh_.param("lineary", linear_y, linear_y);		
  //nh_.param("linearz", linear_z, linear_z);
  //nh_.param("angularz", angular_, angular_);
  
  // buttonTime is used to make sure that one button press is not recognized as multiple presses
  //	essentially it just introduces a delay between when button presses can be detected.
  buttonTime = 0;	// initialize buttonTime
}

void TeleopArdrone::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
  // checks if last button press was more than BUTTONWAIT time
  // if true then the prog checks if a button is being pressed? or was pressed? Not exactly sure how joy works
  if (ros::Time::now().toSec() - buttonTime > BUTTONWAIT){
	  if(joy->buttons[takeoff] == true)
	  	takeoff_pub.publish(empty_);	// if takeoff button is pressed then the takeoff command is sent
	  	
	  if(joy->buttons[land] == true)
	  	land_pub.publish(empty_);	// if land button is pressed then the land command is sent
	  	
	  if(joy->buttons[togglecam] == true)
	  	toggleCam_pub.publish(empty_);	// if togglecam button is pressed then the togglecam command is sent
	  	
	  if(joy->buttons[reset] == true)
	  	reset_pub.publish(empty_);	// if the reset button is pressed then the reset command is sent
	  
	  buttonTime = ros::Time::now().toSec();	// updates buttonTime to current time, resets delay
  }
  
  // controller axis commands, axis vary from -1 to 1
  // pitch axis case loop
  if (joy->axes[pitch] > deadzone)	// if axis > Deadzone then we want to move the drone in pos pitch direction
  	cmd.linear.x = joy->axes[pitch] - deadzone;		// set command: to move forward, proportional to controller axis movement
  else if (joy->axes[pitch] < -deadzone)			// axis < Deadzone, so we want to move backwards
  	cmd.linear.x = joy->axes[pitch] + deadzone;		// set command: move backwards proportional to pitch axis movement
  else												// 0 or within Deadzone, hold current forward pos
  	cmd.linear.x = 0;								// set command: to hold x pos
  
  
  // roll axis case loop, same as above except for roll
  if (joy->axes[roll] > deadzone)
  	cmd.linear.y = joy->axes[roll] - deadzone;
  else if (joy->axes[roll] < -deadzone)
  	cmd.linear.y = joy->axes[roll] + deadzone;
  else
  	cmd.linear.y = 0;
  	
  //cout << id << endl;
  //cout << joy->buttons[deadman_sw] << endl;
  // altitude case loop, same as pitch
  if (id == LOGITECH_ID){
	if ((joy->axes[altitude] > alt_deadzone) && joy->buttons[deadman_sw])
		cmd.linear.z = joy->axes[altitude] - alt_deadzone;
	else if ((joy->axes[altitude] < -alt_deadzone) && joy->buttons[deadman_sw])
	  	cmd.linear.z = joy->axes[altitude] + alt_deadzone;
	else
	  	cmd.linear.z = 0;
  }
  else if(id == XBOX360_ID){
	if (joy->axes[alt_up] < (1 - alt_deadzone))
		cmd.linear.z = (1 - joy->axes[alt_up])/2;	// TO_DO: incorporate alt_deadzone (not important atm)
	else if (joy->axes[alt_down] < 1 - alt_deadzone)
	  	cmd.linear.z = -(1 - joy->axes[alt_down])/2;
	else
	  	cmd.linear.z = 0;
  }
  else{	//do nothing
  }
  
  // yaw case loop, same as pitch
  if (joy->axes[yaw] > deadzone)
  	cmd.angular.z = joy->axes[yaw] - deadzone;
  else if (joy->axes[yaw] < -deadzone)
  	cmd.angular.z = joy->axes[yaw] + deadzone;
  else
  	cmd.angular.z = 0;
  
  
  cmd.angular.x = cmd.angular.y = 0; // not currently used, so hold pos, not sure what these would do
  pub.publish(cmd);		// send control commands
}


int main(int argc, char** argv)
{
  ros::init(argc, argv, "teleop");
  TeleopArdrone teleop;

  ros::spin();
}
